//
// Created by 刘昱杉2020 on 2023/4/14.
//
#include "usart.h"
#include "tim.h"

#include <stdbool.h>
#include <string.h>

#include "adv.h"
#include "motor.h"
#include "detect.h"
#include "oled.h"


#define ADV_TEST_DEBUG

ADV adv1;


void adv_tim_init_all()
{
    HAL_TIM_Base_Start_IT(&htim6);

    HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_ALL); //Tt
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_ALL);//Servo
    HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);	//Encoder
    HAL_TIM_Encoder_Start(&htim8,TIM_CHANNEL_ALL);
}
void adv_pid_init_all()
{
    PID_Init(&adv1.motors[0].motorTt->SpeedPID,200,0.005f,0,10000,10000);
    PID_Init(&adv1.motors[1].motorTt->SpeedPID,200,0.006f,1,10000,10000);

    PID_Init(&adv1.motors[0].motorTt->AnglePID.inner,200,0.005f,0,10000,10000);
    PID_Init(&adv1.motors[1].motorTt->AnglePID.inner,200,0.006f,1,10000,10000);

    PID_Init(&adv1.motors[0].motorTt->AnglePID.outer,0.2f,0,0.5f,30,30);
    PID_Init(&adv1.motors[1].motorTt->AnglePID.outer,0.2f,0,0.5f,30,30);
}
void adv_init()
{
    adv_pid_init_all();
    adv_tim_init_all();
}

#ifdef ADV_TEST_DEBUG
void adv_set_motorSpeed(ADV *adv,float speed0,float speed1)
{
    adv->motors[0].motorTt->TargetSpeed = speed0;
    adv->motors[1].motorTt->TargetSpeed = speed1;
}

void adv_set_targetAngle(ADV *adv,float tt_angle0,float tt_angle1,\
                        float digital_servo_angle, \
                        float bus_servo_angle1,float bus_servo_angle2,float bus_servo_angle3)
{
    Motor_motorTt_StartCalcAngle(&adv->motors->motorTt[0]);
    Motor_motorTt_StartCalcAngle(&adv->motors->motorTt[1]);
    adv->motors->motorTt[0].TargetAngle = tt_angle0;
    adv->motors->motorTt[1].TargetAngle = tt_angle1;

    adv->motors->digital_servo.TargetAngle = digital_servo_angle;
    adv->motors->bus_servo[0].TargetAngle = bus_servo_angle1;
    adv->motors->bus_servo[1].TargetAngle = bus_servo_angle2;
    adv->motors->bus_servo[2].TargetAngle = bus_servo_angle3;


    adv->mode = ADV_MODE_ANGLE;


}
void adv_detect_motor_test()
{
    uint16_t state = Detect_GetState();
//    switch (state)
//    {
//        case straight:
//
//    }
}
void adv_chassis_go_straight()
{
    adv_set_motorSpeed(&adv1,30,30);
}
void adv_chassis_go_left()
{
//    adv_set_motorSpeed(adv1,)
}



#endif